/*
 * This file is part of MonoSettlers.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://opensettlers.codeplex.com/
 */
#include "windows.h"
#include <cmath>
#include "FastPathFinder.h"

#undef GetGValue

typedef struct _PATHFINDER_NODE_{
	double F;
	double H;
	double G;
	int X;
	int Y;
	int Index;
}PATHFINDER_NODE, *LPPATHFINDER_NODE;

#include ".\PriorityQueue.h"
#include ".\OpenCloseMap.h"


class CPathFinder{
private:
	COpenCloseMap* m_ClosedSet;
	COpenCloseMap* m_OpenSet;
	CPriorityQueue* m_OrderedOpenSet;
	LPPATHFINDER_NODE* m_CameFrom;
	PATHFINDER_NODE* m_SearchSpace;

	PATHFINDER_NODE* m_StartNode;
	PATHFINDER_NODE* m_EndNode;
	int* m_UserGrid;
	int m_UserContext;
	int Size;
	int SizeShift;

	void StoreNeighborNodes(LPPATHFINDER_NODE inAround, LPPATHFINDER_NODE* inNeighbors);
	double Heuristic(LPPATHFINDER_NODE inStart, LPPATHFINDER_NODE inEnd);
	double NeighborDistance(LPPATHFINDER_NODE inStart, LPPATHFINDER_NODE inEnd);
	POINT ClipPoint(POINT inPoint);

public:

	CPathFinder(int* inGrid, int inSize);

	~CPathFinder();

	void BeginSearch(POINT inStartNode, POINT inEndNode, int inUserContext);
	void SearchLoop(POINT inRequiredClosedNode);
	double GetGValue(POINT inForNode);
};

FASTPATHFINDER_API CPathFinder* __stdcall PfCreate(int* inGrid, int inSize)
{
	CPathFinder* result = new CPathFinder(inGrid, inSize);

	return result;
}

FASTPATHFINDER_API void __stdcall PfBeginInstancing(CPathFinder* inPathFinder, POINT* inStart, POINT* inEnd, int inContext)
{
	inPathFinder->BeginSearch(*inStart, *inEnd, inContext);
}

FASTPATHFINDER_API double __stdcall PfGetGValue(CPathFinder* inPathFinder, POINT* inForCell)
{
	return inPathFinder->GetGValue(*inForCell);
}

FASTPATHFINDER_API void __stdcall PfRelease(CPathFinder* inPathFinder)
{
	inPathFinder->~CPathFinder();
}

CPathFinder::CPathFinder(int* inGrid, int inSize)
{
	static const double log2Factor = 1.0/log(2.0);

	Size = inSize;
	SizeShift = (int)std::floor((std::log((double)inSize) * log2Factor) + 0.5);

	if((int)std::pow(2, (double)SizeShift) != inSize)
		throw "Size is not a power of two!";

	m_UserGrid = inGrid;
	m_ClosedSet = new COpenCloseMap(inSize);
	m_OpenSet = new COpenCloseMap(inSize);
	m_OrderedOpenSet = new CPriorityQueue();
	m_CameFrom = new LPPATHFINDER_NODE[inSize * inSize];
	m_SearchSpace = new PATHFINDER_NODE[inSize * inSize];

	memset(m_SearchSpace, 0, inSize * inSize * sizeof(PATHFINDER_NODE));

	for(int x = 0; x < inSize; x++)
	{
		for(int y = 0; y < inSize; y++)
		{
			LPPATHFINDER_NODE node = &m_SearchSpace[x + (y << SizeShift)];

			node->X = x;
			node->Y = y;
		}
	}
}

CPathFinder::~CPathFinder()
{
	if(m_ClosedSet != NULL)
		m_ClosedSet->~COpenCloseMap();

	if(m_OpenSet != NULL)
		m_OpenSet->~COpenCloseMap();

	if(m_OrderedOpenSet != NULL)
		m_OrderedOpenSet->~CPriorityQueue();

	if(m_ClosedSet != NULL)
		delete m_ClosedSet;

	if(m_SearchSpace != NULL)
		delete m_SearchSpace;

	if(m_CameFrom != NULL)
		delete m_CameFrom;

	m_ClosedSet = NULL;
	m_OpenSet = NULL;
	m_OrderedOpenSet = NULL;
	m_ClosedSet = NULL;
	m_SearchSpace = NULL;
}

#define SQRT_2 1.41421356

double CPathFinder::Heuristic(LPPATHFINDER_NODE inStart, LPPATHFINDER_NODE inEnd)
{
	return std::sqrt((double)((inStart->X - inEnd->X) * (inStart->X - inEnd->X) + (inStart->Y - inEnd->Y) * (inStart->Y - inEnd->Y)));
}

double CPathFinder::NeighborDistance(LPPATHFINDER_NODE inStart, LPPATHFINDER_NODE inEnd)
{
	int diffX = abs(inStart->X - inEnd->X);
	int diffY = abs(inStart->Y - inEnd->Y);

	switch (diffX + diffY)
	{
		case 1: return 1;
		case 2: return SQRT_2;
		default: 
			return 0;
	}
}

POINT CPathFinder::ClipPoint(POINT inPoint)
{
	if(inPoint.x < 0)
		inPoint.x = 0;

	if(inPoint.y < 0)
		inPoint.y = 0;

	if(inPoint.x > Size - 1)
		inPoint.x = Size - 1;

	if(inPoint.y > Size - 1)
		inPoint.y = Size - 1;

	return inPoint;
}

void CPathFinder::BeginSearch(POINT inStartNode, POINT inEndNode, int inUserContext)
{
	inStartNode = ClipPoint(inStartNode);
	inEndNode = ClipPoint(inEndNode);

	m_StartNode = &m_SearchSpace[inStartNode.x + (inStartNode.y << SizeShift)];
	m_EndNode = &m_SearchSpace[inEndNode.x + (inEndNode.y << SizeShift)];
	m_UserContext = inUserContext;

	m_ClosedSet->Clear();
	m_OpenSet->Clear();
	m_OrderedOpenSet->Clear();

	memset(m_CameFrom, 0, Size * Size * sizeof(LPPATHFINDER_NODE));

	m_StartNode->G = 0;
	m_StartNode->H = Heuristic(m_StartNode, m_EndNode);
	m_StartNode->F = m_StartNode->H;

	m_OpenSet->Add(m_StartNode);
	m_OrderedOpenSet->Push(m_StartNode);
}

double CPathFinder::GetGValue(POINT inForNode)
{
	inForNode = ClipPoint(inForNode);

	SearchLoop(inForNode);

	if(!m_ClosedSet->IsSet(inForNode))
		return -1;

	return m_ClosedSet->Get(inForNode)->G;
}

void CPathFinder::SearchLoop(POINT inRequiredClosedNode)
{
	inRequiredClosedNode = ClipPoint(inRequiredClosedNode);

	if (m_ClosedSet->IsSet(inRequiredClosedNode))
		return;

	LPPATHFINDER_NODE* neighborNodes = new LPPATHFINDER_NODE[8];

	while (!m_OpenSet->IsEmpty())
	{
		if (m_ClosedSet->IsSet(inRequiredClosedNode))
			return;

		LPPATHFINDER_NODE x = m_OrderedOpenSet->Pop();

		m_OpenSet->Remove(x);
		m_ClosedSet->Add(x);

		StoreNeighborNodes(x, neighborNodes);

		for (int i = 0; i < 8; i++)
		{
			LPPATHFINDER_NODE y = neighborNodes[i];
			BOOL tentative_is_better;

			if (y == NULL)
				continue;

			if (m_UserGrid[y->X + (y->Y << SizeShift)] > m_UserContext)
				continue;

			if (m_ClosedSet->Contains(y))
				continue;

			double tentative_g_score = x->G + NeighborDistance(x, y);
			BOOL wasAdded = false;

			if (!m_OpenSet->Contains(y))
			{
				m_OpenSet->Add(y);
				tentative_is_better = true;
				wasAdded = true;
			}
			else if (tentative_g_score < y->G)
			{
				tentative_is_better = true;
			}
			else
			{
				tentative_is_better = false;
			}

			if (tentative_is_better)
			{
				m_CameFrom[y->X + (y->Y << SizeShift)] = x;

				y->G = tentative_g_score;
				y->H = Heuristic(y, m_EndNode);
				y->F = y->G + y->H;

				if (wasAdded)
					m_OrderedOpenSet->Push(y);
				else
					m_OrderedOpenSet->Update(y);
			}
		}
	}
}


void CPathFinder::StoreNeighborNodes(LPPATHFINDER_NODE inAround, LPPATHFINDER_NODE* inNeighbors)
{
	int x = inAround->X;
	int y = inAround->Y;

	memset(inNeighbors, 0, sizeof(LPPATHFINDER_NODE) * 8);

	if (!((x > 0) && (y > 0))) { } else
		inNeighbors[0] = &m_SearchSpace[x - 1 + ((y - 1) << SizeShift)];

	if (!(y > 0)) { } else
		inNeighbors[1] = &m_SearchSpace[x + ((y - 1) << SizeShift)];

	if (!((x < Size - 1) && (y > 0))) { } else
		inNeighbors[2] = &m_SearchSpace[x + 1 + ((y - 1) << SizeShift)];

	if (!(x > 0)) { } else
		inNeighbors[3] = &m_SearchSpace[x - 1 + (y << SizeShift)];

	if (!(x < Size - 1)) { } else
		inNeighbors[4] = &m_SearchSpace[x + 1 + (y << SizeShift)];

	if (!((x > 0) && (y < Size - 1))) { } else
		inNeighbors[5] = &m_SearchSpace[x - 1 + ((y + 1) << SizeShift)];

	if (!(y < Size - 1)) { } else
		inNeighbors[6] = &m_SearchSpace[x + ((y + 1) << SizeShift)];

	if (!((x < Size - 1) && (y < Size - 1))) { } else
		inNeighbors[7] = &m_SearchSpace[x + 1 + ((y + 1) << SizeShift)];
}
